﻿#include "TesMatrix3x3.h"
#include <assert.h>
#include "TesMatrix2x2.h"
#include "TesVector2.h"
#include "TesVector3.h"
#include "TesMatrix4x4.h"
#include "TesEuler.h"
#include "TesQuaternion.h"

void TesMatrix3x3::Zero()
{
	M11_3 = M12_3 = M13_3 = M21_3 = M22_3 = M23_3 = M31_3 = M32_3 = M33_3 = 0.0f;
}

TesMatrix3x3::TesMatrix3x3()
{
	Zero();
}

void TesMatrix3x3::SetMatrix(float m[9])
{
	for (int i = 0; i < 9; ++i)
	{
		this->m3x3[i] = m[i];
	}
}

void TesMatrix3x3::SetMatrix(const TesMatrix2x2& matrix2x2)
{
	Zero();
	M11_3 = matrix2x2.M11_2;
	M12_3 = matrix2x2.M12_2;
	M21_3 = matrix2x2.M21_2;
	M22_3 = matrix2x2.M22_2;
	M33_3 = 1.0f;
}

TesMatrix3x3::TesMatrix3x3(const TesMatrix2x2& matrix2x2)
{
	SetMatrix(matrix2x2);
}

TesMatrix3x3::TesMatrix3x3(float m[9])
{
	SetMatrix(m);
}

TesMatrix3x3& TesMatrix3x3::operator = (const TesMatrix3x3& that)
{
	for (int i = 0; i < 9; ++i)
	{
		this->m3x3[i] = that.m3x3[i];
	}
	return *this;
}

void TesMatrix3x3::Identity()
{
	M11_3 = M22_3 = M33_3 = 1.0f;
	M12_3 = M13_3 = M21_3 = M23_3 = M31_3 = M32_3 = 0.0f;
}

TesMatrix3x3 TesMatrix3x3::operator + (const TesMatrix3x3& that)
{
	TesMatrix3x3 Result;
	for (int i = 0; i < 9; ++i)
	{
		Result.m3x3[i] = this->m3x3[i] + that.m3x3[i];
	}
	return Result;
}

TesMatrix3x3 TesMatrix3x3::operator - (const TesMatrix3x3& that)
{
	TesMatrix3x3 Result;
	for (int i = 0; i < 9; ++i)
	{
		Result.m3x3[i] = this->m3x3[i] - that.m3x3[i];
	}
	return Result;
}

TesMatrix3x3 TesMatrix3x3::operator * (const float& K) const
{
	TesMatrix3x3 Result;
	for (int i = 0; i < 9; ++i)
	{
		Result.m3x3[i] = this->m3x3[i] * K;
	}
	return Result;
}

TesMatrix3x3 TesMatrix3x3::operator / (const float& K)
{
	TesMatrix3x3 Result;
	for (int i = 0; i < 9; ++i)
	{
		Result.m3x3[i] = this->m3x3[i] / K;
	}
	return Result;
}

TesMatrix3x3 TesMatrix3x3::operator * (const TesMatrix3x3& that)
{
	TesMatrix3x3 Result;
	Result.M11_3 = this->M11_3 * that.M11_3 + this->M12_3 * that.M21_3 + this->M13_3 * that.M31_3;
	Result.M12_3 = this->M11_3 * that.M12_3 + this->M12_3 * that.M22_3 + this->M13_3 * that.M32_3;
	Result.M13_3 = this->M11_3 * that.M13_3 + this->M12_3 * that.M23_3 + this->M13_3 * that.M33_3;
	Result.M21_3 = this->M21_3 * that.M11_3 + this->M22_3 * that.M21_3 + this->M23_3 * that.M31_3;
	Result.M22_3 = this->M21_3 * that.M12_3 + this->M22_3 * that.M22_3 + this->M23_3 * that.M32_3;
	Result.M22_3 = this->M21_3 * that.M13_3 + this->M22_3 * that.M23_3 + this->M23_3 * that.M33_3;
	Result.M31_3 = this->M31_3 * that.M11_3 + this->M32_3 * that.M21_3 + this->M33_3 * that.M31_3;
	Result.M32_3 = this->M31_3 * that.M12_3 + this->M32_3 * that.M22_3 + this->M33_3 * that.M32_3;
	Result.M33_3 = this->M31_3 * that.M13_3 + this->M32_3 * that.M23_3 + this->M33_3 * that.M33_3;
	return Result;
}

float TesMatrix3x3::Determinant()
{
	float TempNum[3] = {};
	TempNum[0] = M11_3 * _Cofactor(1, 1);
	TempNum[1] = M12_3 * _Cofactor(2, 1);
	TempNum[2] = M13_3 * _Cofactor(3, 1);
	return (TempNum[0] + TempNum[1] + TempNum[2]);
}

TesMatrix3x3 TesMatrix3x3::Transpose()
{
	TesMatrix3x3 Res;
	Res.M11_3 = M11_3; Res.M12_3 = M21_3; Res.M13_3 = M31_3;
	Res.M21_3 = M12_3; Res.M22_3 = M22_3; Res.M23_3 = M32_3;
	Res.M31_3 = M13_3; Res.M32_3 = M23_3; Res.M33_3 = M33_3;
	return Res;
}

TesMatrix3x3 TesMatrix3x3::SetScale(float x, float y, float z)
{
	Identity();
	M11_3 = x; M22_3 = y; M33_3 = z;
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetScale2D(float x, float y)
{
	Identity();
	M11_3 = x; M22_3 = y;
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetScale2D(const TesMatrix2x2& M2x2)
{
	Identity();
	M11_3 = M2x2.M11_2;
	M22_3 = M2x2.M22_2;
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetRotation2D(float Redus)
{
	Identity();
	M11_3 = cosf(Redus);
	M12_3 = -sinf(Redus);
	M21_3 = sinf(Redus);
	M22_3 = cosf(Redus);
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetRotationXAxis(float Redus)
{
	Zero();
	M11_3 = 1.0f;
	M22_3 = cosf(Redus);
	M23_3 = -sinf(Redus);
	M32_3 = sinf(Redus);
	M33_3 = cosf(Redus);
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetRotationYAxis(float Redus)
{
	Zero();
	M22_3 = 1.0f;
	M11_3 = cosf(Redus);
	M13_3 = sinf(Redus);
	M31_3 = -sinf(Redus);
	M33_3 = cosf(Redus);
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetRotationZAxis(float Redus)
{
	Zero();
	M33_3 = 1.0f;
	M11_3 = cosf(Redus);
	M12_3 = -sinf(Redus);
	M21_3 = sinf(Redus);
	M22_3 = cosf(Redus);
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetRotation(float Redus, const TesVector3& N)
{
	TesVector3 M = N.Normalize();
	float CosSeta = cosf(Redus), SinSeta = sinf(Redus);
	float OneSubCosSeta = 1.0f - CosSeta;
	M11_3 = M.x * M.x * OneSubCosSeta + CosSeta;
	M12_3 = M.x * M.y * OneSubCosSeta + M.z * SinSeta;
	M13_3 = M.x * M.z * OneSubCosSeta - M.y * SinSeta;
	M21_3 = M.x * M.y * OneSubCosSeta - M.z * SinSeta;
	M22_3 = M.y * M.y * OneSubCosSeta + CosSeta;
	M23_3 = M.y * M.z * OneSubCosSeta + M.x * SinSeta;
	M31_3 = M.x * M.z * OneSubCosSeta + M.y * SinSeta;
	M32_3 = M.y * M.z * OneSubCosSeta - M.x * SinSeta;
	M33_3 = M.z * M.z * OneSubCosSeta + CosSeta;
	return *this;
}

TesMatrix3x3 TesMatrix3x3::SetTranslation2D(float x, float y)
{
	Identity();
	M31_3 = x;
	M32_3 = y;
	return *this;
}

void TesMatrix3x3::_GetToArray(float matrix[9])
{
	for (int i = 0; i < 9; ++i)
	{
		matrix[i] = m3x3[i];
	}
}


TesMatrix2x2 GetMatrix2x2(const TesMatrix3x3& matrix, int Row, int Column)
{
	TesMatrix2x2 Res;
	int count = 0;
	for (int i = 1; i <= 3; ++i)
	{
		if (i == Column)
			continue;
		for (int j = 1; j <= 3; ++j)
		{
			if (j == Row)
				continue;
			int temp = PosTOIndex3(j, i);
			Res.m2x2[count] = matrix.m3x3[PosTOIndex3(j, i)];
			count++;
		}
	}
	return Res;
}

float TesMatrix3x3::_Cofactor(int r, int c)
{
	float Factor = powf(-1.0f, float(r + c));
	assert(r >= 1 || r <= 3 || c >= 1 || c <= 3);//三维矩阵取行列不合法
	TesMatrix2x2 Matrix2 = GetMatrix2x2(*this, r, c);
	return Factor * Matrix2.Determinant();
}

TesMatrix3x3 TesMatrix3x3::_AdjoinMatrix()
{
	TesMatrix3x3 Res;
	int count = 0;
	for (int column = 1; column <= 3; ++column)
	{
		for (int row = 1; row <= 3; ++row)
		{
			Res.m3x3[count] = _Cofactor(row, column);
			count++;
		}
	}
	Res = Res.Transpose();
	return Res;
}

TesMatrix3x3 TesMatrix3x3::Inverse()
{
	float Det = this->Determinant();
	assert(Det != 0.0f);  //求逆矩阵的时候行列式的值不能为零,可以用增广矩阵法手动计算
	TesMatrix3x3 Res;
	Res = this->_AdjoinMatrix();
	Res = Res / Det;
	return Res;
}

TesMatrix4x4 TesMatrix3x3::ExtendToMatrix4x4()
{
	TesMatrix4x4 Res;
	Res.Identity();
	Res.SetMatrix(*this);
	return Res;
}

TesMatrix4x4 TesMatrix3x3::Transform2DToMatrix4x4()
{
	TesMatrix4x4 Res;
	Res.Zero();
	Res.M44_4 = 1.0f;
	Res.M41_4 = M31_3;
	Res.M42_4 = M32_3;
	Res.M11_4 = M11_3;
	Res.M12_4 = M12_3;
	Res.M21_4 = M21_3;
	Res.M22_4 = M22_3;
	return Res;
}
TesEular TesMatrix3x3::Euler()
{
	TesEular Res;
	Res.pitch = asinf(-M23_3);
	Res.heading = atan2f(M13_3, M33_3);
	Res.bank = atan2f(M21_3, M22_3);
	return Res;
}

TesQuaternion TesMatrix3x3::Quaternion()
{
	TesQuaternion Res;
	float SqrTempW = M11_3 + M22_3 + M33_3;
	float SqrTempX = M11_3 - M22_3 - M33_3;
	float SqrTempY = M22_3 - M11_3 - M33_3;
	float SqrTempZ = M33_3 - M11_3 - M22_3;

	int BigIndex = 0;
	float BigValve = SqrTempW;

	if (SqrTempX > BigValve)
	{
		BigIndex = 1;
		BigValve = SqrTempX;
	}
	if (SqrTempY > BigValve)
	{
		BigIndex = 2;
		BigValve = SqrTempY;
	}
	if (SqrTempZ > BigValve)
	{
		BigIndex = 3;
		BigValve = SqrTempZ;
	}

	BigValve = sqrtf(BigValve + 1.0f) * 0.5f;
	float K = 0.25f / BigValve;

	switch (BigIndex)
	{
	case 0:
		Res.w = BigValve;
		Res.x = (M23_3 - M32_3) * K;
		Res.y = (M31_3 - M13_3) * K;
		Res.z = (M12_3 - M21_3) * K;
		break;
	case 1:
		Res.x = BigValve;
		Res.w = (M23_3 - M32_3) * K;
		Res.y = (M12_3 + M21_3) * K;
		Res.z = (M31_3 + M13_3) * K;
		break;
	case 2:
		Res.y = BigValve;
		Res.w = (M31_3 - M13_3) * K;
		Res.x = (M12_3 + M21_3) * K;
		Res.z = (M23_3 + M32_3) * K;
		break;
	case 3:
		Res.z = BigValve;
		Res.w = (M12_3 - M21_3) * K;
		Res.x = (M31_3 + M13_3) * K;
		Res.y = (M23_3 + M32_3) * K;
		break;
	}
	return Res;
}

TesMatrix3x3 operator * (const float& K, const TesMatrix3x3& matrix)
{
	return matrix * K;
}

TesVector2 operator * (const TesVector2& v, const TesMatrix3x3& matrix)
{
	TesVector2 Res;
	Res.x = v.x * matrix.M11_3 + v.y * matrix.M21_3 + matrix.M31_3;
	Res.y = v.x * matrix.M12_3 + v.y * matrix.M22_3 + matrix.M32_3;
	return Res; 
}

TesVector3 operator * (const TesVector3& v, const TesMatrix3x3& matrix)
{
	TesVector3 v3;
	v3.x = v.x * matrix.M11_3 + v.y * matrix.M21_3 + v.z * matrix.M31_3;
	v3.y = v.x * matrix.M12_3 + v.y * matrix.M22_3 + v.z * matrix.M32_3;
	v3.z = v.x * matrix.M13_3 + v.y * matrix.M23_3 + v.z * matrix.M33_3;
	return v3;
}





